{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Notebook Setup \n",
    "The following cell will install Drake, checkout the underactuated repository, and set up the path (only if necessary).\n",
    "- On Google's Colaboratory, this **will take approximately two minutes** on the first time it runs (to provision the machine), but should only need to reinstall once every 12 hours.  Colab will ask you to \"Reset all runtimes\"; say no to save yourself the reinstall.\n",
    "- On Binder, the machines should already be provisioned by the time you can run this; it should return (almost) instantly.\n",
    "\n",
    "More details are available [here](http://underactuated.mit.edu/drake.html)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "  import pydrake\n",
    "  import underactuated\n",
    "except ImportError:\n",
    "  !curl -s https://raw.githubusercontent.com/RussTedrake/underactuated/master/scripts/setup/jupyter_setup.py > jupyter_setup.py\n",
    "  from jupyter_setup import setup_underactuated\n",
    "  setup_underactuated()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Global optimization with Sums-of-Squares (the \"Six Hump Camel\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from mpl_toolkits.mplot3d import Axes3D\n",
    "import matplotlib.pyplot as plt\n",
    "from pydrake.all import MathematicalProgram, Solve\n",
    "\n",
    "from underactuated.jupyter import SetupMatplotlibBackend\n",
    "plt_is_interactive = SetupMatplotlibBackend()\n",
    "\n",
    "prog = MathematicalProgram()\n",
    "v = prog.NewIndeterminates(2, \"x\")\n",
    "x = v[0]\n",
    "y = v[1]\n",
    "\n",
    "# This is the famous \"six-hump camel back function\".  It has six local\n",
    "# minima, two of them being global minima.\n",
    "p = 4 * x**2 + x * y - 4 * y**2 - 2.1 * x**4 + 4 * y**4 + x**6 / 3\n",
    "\n",
    "# Find the minimum value by adding a sums of squares constraint, via\n",
    "#   for all x, p(x) >= pmin\n",
    "# which we write as\n",
    "#   p(x) - pmin is sos.\n",
    "pmin = prog.NewContinuousVariables(1, \"pmin\")[0]\n",
    "prog.AddSosConstraint(p - pmin)\n",
    "\n",
    "# Maximize pmin.\n",
    "prog.AddCost(-pmin)\n",
    "\n",
    "result = Solve(prog)\n",
    "assert result.is_success()\n",
    "print(\"Minimum value (lower bound): \" + str(result.GetSolution(pmin)))\n",
    "\n",
    "# Now, let's plot it.\n",
    "fig = plt.figure(figsize=(10, 5))\n",
    "ax0 = fig.add_subplot(121, projection=\"3d\")\n",
    "ax1 = fig.add_subplot(122)\n",
    "xs = np.linspace(-2.2, 2.2, 51)\n",
    "ys = np.linspace(-1.2, 1.2, 51)\n",
    "[X, Y] = np.meshgrid(xs, ys)\n",
    "P = X.copy()\n",
    "for i in range(len(xs)):\n",
    "    for j in range(len(ys)):\n",
    "        P[i, j] = p.Evaluate({x: X[i, j], y: Y[i, j]})\n",
    "ax0.plot_surface(X, Y, P)\n",
    "ax1.contour(X, Y, P, 100)\n",
    "\n",
    "print(\"Minimum sampled value: \" + str(np.min(P)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
